/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : control.hpp
  * Description        :
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Oct 23, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __CONTROL_HPP
#define __CONTROL_HPP

#include "userlib/userlib.hpp"
#include "system/system.hpp"
#include "control/pid/pid.hpp"
#include "flow/flow.hpp"
#include "trajectory/Lissajous.hpp"
#include "trajectory/trajectory.hpp"
#include "trajectory/YawCircle.hpp"

#ifdef __cplusplus

extern "C" {
#endif

#define QUADX		//X字型四轴
//#define OCTAP		//十字型八轴
#define PWM_IDLE	70
#define PID_NUM		14

#define process_standby 0
#define process_takeoff 1
#define process_flying 2
#define process_landing 3

//typedef enum
//{
//	QAUD_MANUAL		= 0x00U,	//手控
//	QAUD_ATTITUDE	= 0x01U,	//姿态
//	QAUD_ALT_HOLD	= 0x02U,	//定高
//	QAUD_POS_HOLD	= 0x03U,	//定点
//	QAUD_TRAJECTORY	= 0x04U,	//航线
//}eQUAD_MODE;

#ifdef __cplusplus
}
#endif

#ifdef __cplusplus
///////////////////////////////////adding//////////////////////////////////////
#include <Eigen>
using namespace Eigen;
///////////////////////////////////////////////////////////////////////////////
using namespace std;

class ATTITUDE_CTRL
{
public:
	ATTITUDE_CTRL();
	~ATTITUDE_CTRL();
	void flight_Mode_Update();
	void motor_Output();

	void Attitude_Ctrl_Init();

	void Attitude_ctrl_run();

	void Ang_ctrl();

	void Height_ctrl();

	void Speed_ctrl();

	void Pos_ctrl();

	void pid_Para_Update();

	void pos_constrain();

private:
	eQUAD_MODE Mode, preMode;
	u8    TakeOff;		//起飞，0:不使能 1:第一次 2:多次 3:结束或无法启用
	u8    Land;			//降落，0:不使能 1:第一次 2:多次 3:结束
	u8    HomeWard;		//一键返航，0:不使能 1:第一次 2:多次 3:结束
//	float WindDir;		//风向
//	float WindAng;		//风力

	bool  LockHead;		//锁尾
	bool  LockHigh;		//锁高
	float HoldYaw;		//锁尾偏航角度
	float InitHigh;		//初始定高高度

	float SetXyz[3];	//位置环目标值
	float SetUvw[3];	//速度环目标值
	float SetAcc[3];	//加速度环目标值
	float SetAng[3];	//角度环目标值
	float SetDAng[3];	//角速度环目标值
	float FeedBackDAng[3];  //角速度环反馈值
	float FeedBackAng[3];	//角度环反馈值
	float FeedBackXyz[3];	//位置环反馈值
	float FeedBackUvw[3];	//速度环反馈值

	float ROL_OUT; //角速度 PID 控制器的输出
	float PIT_OUT;
	float YAW_OUT;
	float Z_OUT;	//位置PID输出
	float X_OUT;
	float Y_OUT;
	float FOREWARD;
	float SetTHR;
	float THROTTLE;		//油门
	float THROTTLE_Offset; //油门保存值

	u8 isPosHold;
	bool LockHeadFirst;
	bool isFirstAltHold;
	bool isFirstProcessPos;
	bool isFirstPosHold;
	bool isFirstTrajectory;
	bool control; 		 //油门大于一定值才开始控制


	uint32_t pid_Para_Update_Time;
	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;

	float trajectory_starttime;
	uint32_t time_now;
	float trajectory_time;
	float trajectory_stoptime;

	sensor_gyro_msg gyro;
	sensor_acc_msg  acc;
	ahrs_euler_msg  ahrsEuler;
	RC_command_msg  rcCommand;
	Motor_PWM_msg   motorPWM;
	Pid_msg         pid_para;
	height_msg      height;
	Control_msg     control_msg; //将需要的数据传到上位机波形图
//	eskf_msg        odometry;
	mahonyAhrs_msg  mahonyAhrs_Ang;
	sensor_gyro_filter_msg gyro_filter; //陀螺仪传感器滤波后的数据
//	landing_msg     LandingMsg;  //自主起降数据，主要是位置err值
	ekf_uwb_pos_vel_msg ekf_uwb_pos_vel;
	kal_uwb_pos_vel_msg kal_uwb_pos_vel;
	uwb_pos_msg uwb;
	flow_msg flow;
	TRAJECTORY_LISSAJOUS lissajous;
	YAW_CIRCLE yaw_circle;

//////////////////////adding///////////////////////////////
	Matrix<float,3,3> J;                //建模力矩
	Vector3f angleRate;
	Matrix<float, 3, 3> angleRate_;
	Vector3f D;
	Vector3f moments;                   //转矩控制器输出
	float F[3];
	float X1_[3];
	float X2_[3];
	float beta1[3];
	float beta2[3];
	float Wn[3];
	float Wn_last[3];
	float dWn[3];
	float dWn_last[3];
	float ddWn[3];
	float ddWn_last[3];
	float dddWn[3];

	float Wb = 5.0f;
	float Wnc = 0.0f;
	float h = 0.01f;
	float dt = 0.001f;
	float time = 0.0f;

	addingMoments addingmoments;

///////////////////////////////////////////////////////////

	mocap_pos_msg mocap;

	float pos_old[3];
	float Xyz_offset[3];

	Matrix<float,8,1> plan_x;
	Matrix<float,8,1> plan_y;
	uint32_t plan_count;

	int rolling_liftoff_mode;
	int TakeOff_Count;
	bool isFirstLanding;
	float LandingHeight;
	float setHeight;

	Vector2f xyoutput;
	Matrix<float,2,2> yaw_R;
};


#endif



#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
